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SUMMARY 

SINSl  is  a  simulation  of  a  strapdown  INS,  written  as  a  Fortran 
subroutine  to  be  called  at  intervals  by  another  program.  Sensor  errors,  system 
initialization  characteristics,  and  navigation  algorithm  operation  may  be 
selected  by  the  User.  Each  call  corresponds  to  the  passage  of  a  set  time 
interval,  and  the  calling  program  provides  environmental  dynamics 
information  about  the  angular  velocity  of,  and  specific  force  on,  the  simulated 
INS.  As  required  by  the  User,  the  calling  program  obtains  from  SINSl,  the  INS 
outputs  of  position,  attitude,  velocity,  and  other  data.  An  example  of  a 
calling  program  is  also  described. 
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1. 


INTRODUCnON 


SINSl  is  a  computer  simulation  model  of  a  strapdown  inertial  navigation 
system  (INS).  It  is  written  as  a  Fortran  subroutine  which  is  to  be  called  at  intervals 
by  some  other  program,  which  may  be  referred  to  as  the  handler  program.  Any 
simulation  of  an  INS  requires  a  simulated  environment  for  the  system,  and  this 
arrangement  allows  the  INS  simulation  to  be  separated  from  that  of  its  environment, 
thus  increasing  the  model’s  utility.  As  an  example,  Section  6  of  this  document 
contains  a  description  of  a  handler  program  which  operates  SINSl  using  the  outputs 
of  a  flight  profile  generator,  -  a  program  which  simulates  an  idealised  airborne 
environment. 

Each  call  to  SINSl  corresponds  to  the  passage  of  a  set  (real  world)  time 
interval,  and  the  handler  provides  environmental  dynamics  information  about  the 
angular  velocity  of,  and  specific  force  on,  the  simulated  INS.  At  required  intervals, 
the  handler  obtains  from  SINSl  the  INS  outputs  such  as  position,  attitude,  and 
velocity. 

The  simulated  INS  model  has  two  parts  -  the  sensors  segment  and  the 
navigator  segment.  Angular  velocity  and  specific  force  outputs  to  SINSl,  in  nominal 
INS  ("Body")  coordinates,  are  sampled  and  integrated  by  the  sensors  segment,  giving 
"ideal"  rate-integrating  gyro  and  incremental-velocity  accelerometer  outputs. 
Optionally,  SINSl  will  accept  data  as  integrated  (in  Body  axes)  angular  velocity  and 
specific  force  components.  The  "ideal"  sensor  outputs  are  then  corrupted  in  the 
sensor  segment  with  sensor  characteristics  and  errors  including  misalignment,  scale 
factor  errors,  fixed  biases,  and  quantization. 

The  navigator  segment,  which  produces  the  outputs  of  position,  velocity,  and 
attitude,  uses  a  split  frame  algorithm,  partitioned  into  fast,  intermediate,  and  slow 
update  rate  segments,  the  repetition  rates  of  which  may  be  preset  to  suit 
requirements.  Quaternions  are  used  for  body  attitude  and  angular  position  relative 
to  Earth,  and  the  navigation  axes  are  wander  azimuth/down.  Vertical  channel 
navigation  may  be  either  pure-inertial  or  set  to  follow  input  altitude.  Altitude 
smoothing  is  not  incorporated.  Levelling  and  alignment  of  the  navigator  may  be 
performed  either  by  specifying  errors  in  the  initial  position,  velocity,  and  attitude,  or 
by  using  a  simple  "one  shot"  coarse  alignment  routine  which  uses  the  (unquantizccl 
and  stationary)  sensor  outputs  to  estimate  level  and  north. 
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At  any  time  during  a  simulation  run,  the  calling  program  may  change  the 
sensor  error  characteristics;  it  may  also  make  changes  ("tilts")  to  the  attitudes  of  the 
navigator’s  analytical  axes  at  any  time. 

As  output,  SINSl  returns  to  the  calling  program  with  time  tagged  attitude, 
velocity,  and  position.  Attitude  is  available  as  Geographic  to  Body  Euler  angles,  also 
as  Wander  to  Body  quaternion  and  direction  cosine  matrix.  Position  is  available  as 
longitude  and  latitude  angles,  also  as  Earth  to  Wander  quaternion  and  direction 
cosine  matrix.  Velocity  relative  to  Earth  is  available  in  Wander  and  Geographic 
coordinates.  See  Appendix  1  lor  coordinate  frames  and  notation. 

2.  SENSORS  SEGMENT 

A  "sensoi  cyclt"  is  defined  as  the  process  whereby  simulated  sensor  output  is 
generated,  updated  within  SINSl  and  made  available  for  sampling  by  t/ie  navigator 
segment. 

Ideally,  gyroscope  outputs  are  the  integral  of  angular  velocity  about  the 
gyroscope  sensitive  axis,  over  a  set  period  of  time.  These  have  the  dimensions  of 
angular  increments.  Similarly,  accelerometer  outputs  are  the  integral  of  specific 
force  along  the  accelerometer  sensitive  axis,  over  the  period,  and  have  the  dimension 
of  velocity  increments.  The  sensors  model  is  provided  with  samples  of  angular 
velocity  and  specific  force  from  the  calling  program.  These  samples  are  integrated 
by  the  Simpsons  Rule  method  to  simulate  ideal  sensors’  outputs.  Simpsons  Rule 
requires  two  samples  per  integration,  which  correspond  with  the  mid-point  and  end  of 
a  sensor  cycle.  A  sensor  cycle  is  therefore  completed  after  each  second  call  to 
SINSl:  the  calls  are  for  the  mid  point  and  end  of  the  cycle.  Simpsons  Rule  also 
requires  a  sample  for  the  beginning  of  the  sensor  cycle,  which  is  usually  the  previous 
cycle’s  end  value:  for  a  time  interval  from  T  to  (T+h),  the  integral  of  a  function  x(t) 
is  given  by: 


(T+h) 

x(t)dt  =  ^hl  x(T)  +  4x(T+-^h)  +  x(T+h)  1. 
b  Z 


SINSl  can  accurately  accommodate  step  changes  in  specific  force,  provided 
they  are  timed  to  occui-  at  the  beginning  of  a  sensor  cycle.  In  this  case,  an  extra  call 
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is  made  with  the  new  initial  values,  with  a  flag  set  so  no  other  processing  takes 
place. 

Optionally,  SINSl  will  accept  values  of  integrated  (in  Body  axes)  angular 
velocity  and  specific  force,  in  which  case  the  above  integration  is  not  performed,  and 
a  sensor  cycle  is  completed  on  every  call. 

2.1.  Sensor  Errors 

Once  per  sensor  cycle,  these  "perfect"  sensor  outputs  are  corrupted  with 
sensor  characteristics  and  errors,  which  at  present  include  misalignments,  scale 
factor  errors,  fixed  biases,  and  quantization. 

Misalignments  are  defined  for  each  sensor  set  by  a  3  x  3  matrix  which 
specifies  each  individual  sensor’s  response  to  inputs  along  the  other  axes,  as  well  as 
its  own  sensitive  axis.  Responses  of  gyroscopes  to  specific  forces  or  of 
accelerometers  to  angular  velocity  or  acceleration  could  be  similarly  included,  but 
are  not  at  this  stage. 

Scale  factor  errors  are  included  by  multiplying  each  sensor  output  by  an  error 
factor,  (Alternatively  these  may  be  included  with  the  misalignment  matrices  if 
required.) 

Fixed  biases  are  included  by  adding  to  each  sensor  output  an  amount 
corresponding  to  the  zero  offset  multiplied  by  the  sensor  cycle  time. 

Conservative  quantization  may  be  included.  The  sensor  output  is  truncated  to 
an  integer  number  of  quanta,  and  the  residual  is  saved  to  be  added  (before 
quantization)  to  the  sensor  output  in  the  next  sensor  cycle. 

3.  NAVIGATOR  SEGMENT 

The  navigator  model  is  based  upon  a  split-frame  algorithm  described  in 
reference  (1),  with  certain  changes  and  additions.  Whereas  (1)  used  the 
North/Elast/Down  (G)  frame  for  navigation,  with  position  maintained  directly  in 
Longitude  and  Latitude  angles,  this  model  maintains  its  position  via  the  Earth  to 
Navigation  axes  quaternion,  and  the  Navigation  frame  is  the  Wander  azimuth/Down 
(W)  frame.  Sensor  compensation  is  not  included;  for  present  purposes,  errors  at 
sensor  level  are  sufficient.  The  fast  segment  samples  tne  sensors’  outputs  when 
required,  updates  the  attitude  quaternion  for  the  body’s  absolute  rotation  and 
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accumulates  velocity  increments  caused  by  specific  force,  in  body  axes.  This 
segment  has  a  choice  of  orders  of  accuracy  for  the  attitude  updating  and  body  axis 
velocity  accumulation  calculations.  The  intermediate  segment  updates  the  attitude 
quaternion  for  absolute  rotations  caused  by  Earth  rotation  and  transport.  It  also 
transforms  the  body  axes  velocity  increment  into  navigation  axes,  compensates  for 
velocity  increment  from  gravity,  and  does  the  position  update.  In  the  slow  segment, 
slowly  varying  Earth-related  quantities  are  updated. 

Since  some  applications  have  low  dynamics,  and  may  be  required  to  run  for 
extended  periods,  there  are  optional  higher  order  algorithm®  for  attitude  updating 
and  velocity  accumulation  calculations.  These  impose  slightly  heavier  computer 
loadings  per  iteration  of  the  navigator  algorithm,  but  allow  lower  iteration  rates  for 
a  given  accuracy,  provided  the  frequency  content  of  the  environment  is  appropriate. 

3.1.  Split  Frame  Algorithm 

For  the  sake  of  completeness  of  this  report,  the  algorithm  will  now  be  briefly 
summarized. 

The  Body  frame  rotation  rate  may  be  several  hundred  degrees  per  second  -  far 
greater  than  the  Navigation  frame  rate,  even  in  a  high  speed  aircraft.  Changes  in 
gravity,  and  effects  of  Earth  rotation  and  curvature  are  functions  of  the  position  of 
Navigation  axes  relative  to  Earth:  although  they  change  slowly,  their  effects  on  the 
accelerometers  change  at  Body  rate.  The  split  frame  mechanisation  performs  Body 
axes  related  calculations  at  a  fast  iteration  rate  (the  "Fast  Segment")  in  Body 
coordinates,  and  Navigation  axes  related  calculations  at  a  slower  rate  (the 
"Intermediate  Segment")  in  Navigation  coordinates.  At  what  may  be  a  yet  slower 
rate  (the  "Slow  Segment"),  certain  slowly  varying  quantities  such  as  local  Earth  radii 
and  gravity,  are  calculated. 

The  attitude  part  of  the  algorithm  updates  the  attitude  quaternion  in  the  Fast 
Segment  for  attitude  changes  of  the  Body  relative  to  the  Inertial  frame,  using 
gyroscope  outputs.  In  the  Intermediate  Segment,  the  attitude  quaternion  is  updated 
to  account  for  rotation  of  the  Navigation  frame  relative  to  the  Inertial  frame;  this 
update  is  calculated  from  position  changes  and  Earth  rotation. 

The  basis  of  the  navigation  part  of  the  algorithm  is  that  changes  in  velocity  or 
position  caused  by  specific  force  may  be  calculated  separately  from  changes  caused 
by  gravitation  (mass  attraction).  The  actual  change  is  the  sum  of  these.  Velocity 
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increments  caused  by  specific  force  are  accumulated  in  the  Fast  Segment.  In  the 
Intermedia!  Segment  these  are  transformed  into  Navigation  coordinates,  and  are 
added  to  c.  alated  velocity  increments  caused  by  gravitation.  Position  updating  is 
then  performed. 

Coordinate  transformations  between  Earth,  Geographic,  and  Wander  Azimuth 
frames  are  described  in  Appendix  3.  Coordinate  transformations  between  the 
Navigation  frame  and  the  Body  frame  are  described  in  Appendix  4.  The  relationships 
in  Appendix  4  apply,  whatever  frame  is  used  as  the  Navigation  frame:  in  SINSl,  the 
Wander  Azimuth  frame  is  used.  Angular  velocity  relationships  between  Inertial, 
Earth,  Geographic,  and  Wander  frames  are  derived  in  Appendix  5. 

3.2.  Fa.st  Segment  Calculations 

The  "Fast  Period"  is  defined  as  the  time  between  outputs  from  the  fast 
segment.  The  fast  segment  requires  sensor  output  data  at  the  mid-point  and  end¬ 
point  of  its  period.  Sensors’  outputs  are  sampled  at  each  entry  to  the  fast  segment, 
but  the  calculations  are  only  performed  at  each  second  entry.  The  fast  period  is 
therefore  twice  the  time  between  calls  to  the  fast  segment  and  is  an  even  multiple 
of  the  sensor  cycle  period. 

3.2.1.  Attitude  Updating 

Gyroscopes  incremental  sample  sets  0^  and  0^  at  mid-point  and  end-point 

of  the  fast  period  are  used  to  estimate  the  rotation  vector  : 

1 15 


-IB 


=  0 


+  0  + 
— e 


X  0  ) 

— e 


a  derivation  of  this  is  available  in  reference  (1),  appendix  (6). 

From  this  is  calculated  the  updating  quaternion  QU  for  attitude  changes 
relative  to  inertial  space  (for  example,  to  3rd  order): 


--  ( $  $  )  I 

8  -IB  -IB 


48^-IB’-IB^  ^-IB 


QU  =  [  I 
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The  attitude  quaternion  QWB  is  updated  (see  Appendix  2)  for  this  rotation: 


QWB  ^  QWB(*)QU 

3.2.2.  Body  Axes  Velocity  Accumulation 

'fhe  equation  for  velocity  _V  caused  by  specific  force  SF  at  any  instant  is: 

V  =  SF  -  [^jg]xV 

A  simple  solution  for  this  is  available  as  in  reference  (1),  appendix  (9).  For  this 
model,  an  optional  4th  order  Runge-Kutta  solution  is  also  available,  offering  higher 
accuracy  but  for  a  greater  computing  burden. 

3.3.  Intermediate  Segment  Calculations 

The  "Intermediate  Period"  is  defined  as  the  time  between  calls  to  the 
intermediate  segment.  This  segment  takes  the  output  from  the  fast  segment,  plus 
height  data  from  the  calling  program,  for  its  input.  It  updates  its  output  on  each 
call,  so  the  intermediate  period  may  be  any  integer  multiple  of  the  fast  period,  i.e., 
an  even  multiple  of  the  time  between  calls  to  the  fast  segment.  The  call  is  defined 
to  be  at  the  end  of  the  segment. 

3.3.1.  Attitude  Updating 

Since  the  previous  entry  to  this  segment,  the  fast  segment  has  updated  the 
attitude  quaternion  for  absolute  rotation.  Now  the  quaternion  is  updated  for 
navigation  axes  rotations  as  described  in  reference  (1),  appendix  11. 

From  the  previous  entry  to  this  segment,  there  is  a  (1st  order)  estimate  of 
Navigation  axes  rotation  vector  since  that  time.  A  third  order  estimate  of 

the  corresponding  quaternion  is  made:  the  updating  quaternion  QU  is  the  inverse  of 
this.  QWB  is  then  updated: 


QWB  QU(  *  )QWB 
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3.3.2.  Velocity  Integration 

W 

The  direction  cosine  matrix  Cg  for  the  Body  to  Wander  transformation  is 
calculated  from  QWB .  The  velocity  increment  6Vp  from  the  fast  segment  is 
transformed  from  B  to  W  coordinates; 


W  W  B 

[5Vp]"  =  Cg  [Wp] 


,B 


the  [  are  then  set  to  zero  for  the  next  cvcle.  From  the  previous  entry  to 


this  segment,  there  is  an  estimate  of  the  velocity  increment  due  to  gravitation 

W 

which  is  auded  to  [AV„]  ,  together  with  a  gravity  support  correction 


W 

[5V_]  , 


g 

(see  appendix  6),  to  give  the  total  velocity  increment  [  5V„,,] 

W  ™ 

Total  velocity  [Vp^l  is  then  updated  by  adding  the  increment. 


W 


relative  to  E^rth. 


3.3.3.  Position  Update 

Angular  velocity  of  W  relative  to  E  frame,  in  W  coordinates  is  given  by 


W  W 


where 


D  = 


-DX  D2  0 
-D1  DX  0l 

0  0  oi 


The  components  of  D  are  calculated  by  the  Slow  segment.  They  are  functions  of 
Elarth  radius.  Latitude,  and  Wander  angle  (see  appendix  5). 

Position  is  maintained  by  the  Earth  to  Wander  frame  quaternion  QEW.  This  is 
updated  with  the  quaternion  QU  representii  g  the  rotation  of  the  Wander  frame 
relative  to  Elarth  during  the  intermediate  segment. 

If  T  is  the  intermediate  period,  rotation  of  W  frame  relative  to  E  frame  is: 


r-  .W  _  ,  ,W 


.T 
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—  1  W 

Updating  quaternion  of  above  to  1st  order  is:  QU  =  1, 

Position  update: 


QEW  QEW(  •  )QU 

3.3.4.  Values  for  next  Intermediate  cycle 

Absolute  rotation  of  Wander  frame  during  T  is: 


W  W 


Velocity  increment  due  to  gravitation: 


W  W  W  1  W 


w  w 

Values  of  [g^]  and  are  calculated  in  the  Slow  segment. 


3.4  Slow  Segment  Calculations 

This  segment  is  used  for  calculation  of  slowly  varying  quantities  related  to  the 
systems  position  relative  to  Elarth.  These  require  parts  of  the  E  to  W  direction 
cosine  matrix  CEW,  which  are  calculated  from  the  QEW. 

Quantities  updated  in  this  segment  are; 


Components  of  the  D  matrix  (see  appendix  5), 


Angular  velocity  of  E  relative  to  I  frame,  in  W  coordinates: 

Gravity  support  correction  (see  appendix  6), 


Vertical  component  of  gravity  g3  (n.b.,  in  a  fast  aircraft  application,  this 
would  perhaps  be  in  the  Intermediate  segment).  This  is  calculated  from  the 
relationship: 
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g3  =  Ge.d  +  GL.sindLAT))  -  (GA.H)/Re 

where  Ge  is  gravity  at  equator,  GL  is  a  latitude  factor,  GA  is  an  altitude 
factor,  H  is  altitude  (metres,  +ve  up),  and  Re  is  Earth  equatorial  radius,  in 
metres. 

3.5  One  shot  coarse  alignment 

This  facility  is  described  in  appendix  7.  The  simulated  sensor  outputs, 
including  sensor  errors  but  without  quantization  effects,  are  calculated  for  the 
system  stationary  at  the  true  initial  attitude.  The  simulated  accelerometer  outputs 
arising  from  gravity  support  are  then  used  to  calculate  the  quaternion  of  the  rotation 
from  "level"  to  Body  axes.  This  is  used  to  calculate  the  simulated  levelled  gyro 
outputs  arising  from  Earth  rotation,  and  these  are  used  to  establish  the  heading  of 
the  levelled  axes  and  hence  the  G  to  B  axes  relationship. 

This  is  a  rather  crude  method  which  would  only  be  useful  in  a  real  system  for 
making  an  initial  coarse  alignment  estimate. 

3.6  Introduction  of  "TUt  changes"  during  a  run 

These  may  be  specified  relative  to  either  the  Body  or  the  Geographical  axes  in 
terms  of  the  yaw,  elevation,  and  bank  angles,  in  that  order.  The  effect  is  a  change  in 
the  Wander  to  Body  reference  quaternion  QWB.  Tilts  are  regarded  as  positive  in 
the  sense  <systems  previous  estimate  of  Body  attitude>  to  <systems  new  estimate  of 
body  attitude>. 

3.6.1  Relative  to  Body  axes 

This  is  effectively  equivalent  to  the  reception  of  spurious  gyro  data.  The 
quaternion  of  the  tilt  QT  is  calculated  (as  for  QNB  in  appendix  4.2)  from  the  angles, 
and  the  new  QWB  is  obtained: 


QWB  QWB(*  )Qr 
3.6.2  Relative  to  Geographic  axes 

Here,  the  tilt  is  specified  as  heading,  elevation,  and  bank  (of  the  body)  in 
Geographic  axes  angles.  For  example,  in  a  Body  heading  West  with  elevation  axis 
level,  unit  tilt  about  North  would  appear  as  unit  tilt  about  the  elevation  axis. 


In  terms  of  the  G  to  B  and  the  W  to  B  relationships,  this  is  equivalent  to  an 
equal  and  opposite  tilt  rotation  of  the  G  relative  to  B  axes,  with  the  G  to  W 
relationship  kept  constant. 

Consider  the  "true"  G  reverse  tilted  to  G’  and  the  "true"  W  reverse  tilted  to 
W’.  The  required  Wander  to  Body  quaternion  after  tilting  has  the  value  of  QW’B, 
given  by  (see  appendix  2.2): 


QW’B  =  [[  QW’G’(*)QG’G  ](*)QGW  ](*)QWB  ]. 

QG’G  is  obtained  from  the  specified  tilt  (as  QNB  in  appendix  4.2). 
QGW  is  rotation  through  wander  angle,  and: 


QW’G’  =  QWG  =  (QGW) 


(QGW  is  Q3  in  appendix  3.2) 


QWB  is  the  previous  (before  tilt)  value. 

QWB  ’  is  the  new  (after  tilt)  value  for  QWB  . 

4.  PROGRAM  IMPLEMENTATION 

The  executive  routine  for  the  program  is  called  SINSl,  shown  in  figure  1.  This 
performs  little  more  than  error  checking,  timekeeping,  and  making  calls  to  the  major 
subroutines  SENSORS  and  NAVEXEC,  shown  in  figures  2  and  3. 


4.1  Sensors  Segment 

Routine  SENSORS  generates  simulated  integrating  gyroscope  and 
accelerometer  outputs,  as  described  in  Part  2.  If  the  entry  is  at  time  zero,  the 
sensor  initialisation  routine  SENSINI  is  called,  and  no  further  processing  takes 
place.  If  sensor  errors  are  to  be  re-initialised  at  a  particular  entry,  SENSINI  is 
called,  followed  by  a  normal  run  through  the  rest  of  the  routine. 

Output  of  SENSORS  is  in  the  form  of  "buffers"  containing  all  the  sensor 
outputs.  As  they  are  integrating  sensors,  the  outputs  are  added  to  the  buffer 
contents  as  they  are  generated:  this  allows  the  output  sampling  to  take  place  at  any 
time,  provided  the  sampling  routine  resets  the  buffers  contents  to  zero. 
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Routine  SENSINI  calls  a  routine  SENREAD,  which  reads  the  sensor 
ohuracteristics  data  file,  or,  if  this  file  is  absent,  sets  the  default  values.  It  does 
some  error  checking  and  sets  up  various  initial  values  as  required. 

4.2  Navigator  Segment 

The  executive  routine  for  this  segment  is  NAVEXEC,  whose  function  is  simply 
to  decide  which,  if  any,  of  the  major  navigator  routines  are  to  be  called  on  that 
entry,  and  to  call  the  routines.  These  routines  are: 

NAVINIT,  the  initialisation  routine 

NAVFAST,  the  "fast  segment"  routine 

NAVIMED,  the  "intermediate  segment"  routine 

NAVSLOW,  the  "slow  segment"  routine 

NAVOUTS,  the  output  generation  routine 

Routine  NAVINIT  is  only  called  on  the  first,  initialisation  call  to  SINSl.  It 
calls  a  routine  NAVREAD,  which  reads  the  navigator  data  file,  or,  if  this  file  is 
absent,  sets  the  default  values.  NAVINIT  uses  this  data,  together  with  the  data 
passed  with  the  call  to  SINSl,  to  set  up  initial  values  in  all  the  variables  used  in  the 
navigator  segment.  If  the  calling  program  requires  a  one-shot  alignment,  NAVINIT 
calls  the  alignment  routine  SNAPALYN,  described  in  part  3.5. 

Routines  NAVFAST,  NAVIMED,  and  NAVSLOW  perform  the  fast, 
intermediate,  and  slow  segment  calculations  as  described  in  parts  3.2  to  3.4. 

Routine  NAVOUTS  writes  the  output  array.  The  angular  position,  wander,  and 
attitude  data  are  obtained  from  the  quaternion  elements  as  shown  in  appendices  3 
and  4.  If  the  latitude  or  the  elevation  approaches  +  or  -  90  degrees,  a  warning  flag  is 
set.  In  either  case,  valid  position  and  attitude  output  data  is  still  available  from  the 
appropriate  quaternion  or  direction  cosine  elements. 

5.  SINSl  OPERATION 

SINSl  is  a  Fortran  F77  subroutine  which  is  invoked  by: 

CALL  SINSl (IIO,DIN,DOUT,CIO), 

where  BO  is  an  array  of  input  and  output  integer  variables,  DIN  and  DOUT  are  arrays 
of  input  and  output  double  precision  (REAL*8)  variables  respectively,  and  CIO  is  an 
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array  of  character  strings  for  passing  filenames.  Double  precision  is  used  for  all  real 
variables  in  SINSl. 

Two  data  files  may  be  called  by  SINSl  during  initialisation,  containing 
initialisation  data  for  the  sensors  and  the  navigator  segments.  Formats  for  these 
files  are  given  below.  If  required,  the  files  are  not  called,  and  default  values  used. 
Further  data  are  passed  to  the  routine  as  arguments  to  the  call,  both  at  initialisation 
and  during  the  run. 

The  calling  program  is  to  be  part  of  a  time  stepping  simulation,  where  the 
time  keeping  includes  an  integer  counter  of  fixed  period  time  steps.  SINSl  must  be 
called  at  each  increment  of  this  counter,  with  the  appropriate  input  data.  For 
initialisation  it  must  be  called  with  the  counter  at  zero.  Output  is  available,  except 
as  stated  below,  from  any  call  including  at  time  zero,  although  there  may  be  some 
latency  in  subsequent  output  data,  because  this  is  updated  during  the  intermediate 
segment  of  the  navigation  algorithm. 

Elements  of  HO,  CIO,  DIO  and  DOUT  are  listed  in  Tnl:les  1,  2  and  3. 

5.1  Lutialisation  call 

nO(l)  -  SIMSTEPCTR  -  must  have  a  value  of  zero  for  the  initialisation  call, 
i.e.  time  =  0,  as  this  is  used  by  SINSl  to  trigger  its  initialisation  processes. 

110(2)  -  SENSFLAG  -  must  have  a  value  of  1  or  2  in  this  call.  This  indicates  to 
SINSl  whether  input  data  during  the  run  will  be  angular  velocity  and  specific  force 
(2),  or  their  integrals  (1),  In  the  former  case,  the  sensor  cycle  time  is  twice  the  step 
time,  and  every  other  call  will  be  used  only  to  pass  input  data  at  the  mid  point  of  the 
sensor  cycle;  in  the  latter  case,  the  sensor  cycle  is  equal  to  the  step  time,  and  sensor 
output  is  updated  on  every  call. 

nOO)  -  ALYNFLAG  -  must  have  a  value  of  0  or  1  on  this  call.  This  is  used  to 
tell  SINSl  how  the  Wander  to  Body  quaternion  is  to  be  initialised.  If  it  is  1,  a  coarse 
one  shot  alignment  will  be  carried  out  using  the  (unquantized  and  unmoving)  sensors’ 
outputs.  If  it  is  0,  SINSl  will  use  the  values  of  heading,  bank,  and  elevation  angles 
passed  in  DIN. 

110(4)  -  HGIITFLAG  -  must  have  a  value  of  0  or  1  in  any  call.  If  0,  the  value 
of  height  passed  in  DIN  will  be  used  by  SINSl,  and  the  vertical  component  of  velocity 
output  will  be  set  to  zero.  If  1,  the  vertical  channel  is  free  inertial  (and  therefore 
unstable). 


110(5)  -  OUTFLAG  -  should  be  0,  1  or  2  in  any  call.  If  0,  no  output  is  provided 
in  DOUT.  If  1,  partial  output  is  provided,  otherwise  full  output  is  provided,  as 
described  later. 

110(6)  -  UNI  -  is  passed  as  the  Fortran  unit  number  for  SINSl  to  assign  to  each 
data  file  whilst  being  read.  Only  one  number  is  required  because  only  one  file  is  open 
at  any  time. 

110(7)  is  an  output  warning  flag:  if  (and  only  iD  full  output  has  been  requested, 
110(7)  is  returned  set  to  1  if  the  system  is  near  a  pole,  2  if  the  system  is  near  gimbal 
lock  (elevation  +/-90  degrees),  and  3  if  both.  If  either  of  these  occurs,  the  undefined 
angles  are  set  to  zero.  If  neither  occurs,  110(7)  is  returned  set  to  0.  If  full  output 
has  not  been  requested,  110(7)  is  undefined. 

110(8)  -  SULKS  -  is  an  output  flag  for  SINSl  at  any  call.  If  operations  are 
normal,  it  will  be  set  to  0.  If  a  ’fatal’  problem  is  encountered  in  initialisation  or 
running,  SINSl  will  write  a  message  to  the  terminal  and  set  110(8)  to  1.  SINSl  will 
then  go  dormant:  subsequent  calls  to  SINSl  will  be  returned  with  no  processing,  and 
no  valid  output,  other  than  110(8)  being  set  to  1. 

CIO(l)  is  a  character  string  (up  to  80  characters),  which  contains  the  full 
pathname  of  the  sensor  data  file  from  which  SINSl  is  to  read  sensor  characteristics. 
If  CIO(l)  is  a  null  string,  SINSl  will  default  to  assume  error  free  sensors. 

CIO(2)  is  a  similar  string  containing  the  full  pathname  of  the  navigator  data 
file.  If  CIO(2)  is  a  null  string,  SINSl  will  use  its  own  default  values. 

The  DIN  are  all  inputs  to  SINSl: 

DIN(l)  to  DIN(6)  are  for  Body  axes  angular  velocity  and  specific  force,  or 
their  integrals.  If  SINSl  is  to  use  integrated  input  data  (SENSFLAG  =  2),  these 
variables  are  not  used  on  this  call  and  need  not  be  set.  Otherwise,  the  variables  must 
be  set  up  with  their  values  at  time  =  0,  as  initial  values  for  the  sensors’  integration 
process. 

DIN(7)  is  HEIIGHT  at  any  call. 

DIN(8)  -  SIMSTEPSEC  -  is  the  length  of  time  in  the  simulation  corresponding 
to  an  increment  of  SIMSTEPCTR. 
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DIN(9)  to  DIN'(  18)  are  for  initial  conditions  used  by  the  navigator  segment: 
they  are  not  necessarily  the  "true"  values.  DIN(16)  to  DIN(18),  (bank,  elevation, 
heading  angles)  need  not  be  defined  if  a  one  shot  alignment  is  to  be  used  (if 
ALYNFLAG  is  1). 

DIN(19)  to  DIN(28)  are  allocated  for  the  "true"  (or  "reference")  versions  of  the 
above.  SINSl  uses  some  of  these  for  the  one  shot  alignment. 

5.2  In-Run  calls 

nOd)  contains  the  current  value  of  SIMSTEPCTR. 

110(2)  may  take  the  value  3,  4,  5  or  6.  Other  values  will  be  ignored.  If  3  to  6 
are  not  required,  it  is  suggested  that  some  value  be  written,  say,  1  or  2,  because 
110(2)  is  tested  every  time. 

If  110(2)  is  set  to  3,  the  call  may  be  used  as  an  additional  call  to  accommodate 
step  changes  in  specific  force  (step  changes  in  angular  velocity  are  assumed 
impossible),  and  the  user  wishes  to  reset  the  specific  force  recorded  for  the 
beginning  of  the  current  sensor  cycle.  In  this  case  the  new  values  of  specific  force 
must  be  in  DIN(4)  to  DIN(6).  SINSl  then  returns  without  further  processing.  Output 
is  not  available  from  this  call. 

If  110(2)  is  set  to  4,  the  sensor  error  characteristics  are  reset  as  from  the  start 
of  the  current  sensor  cycle.  In  this  case,  a  new  sensor  data  file  is  required,  and 
values  must  be  available  in  110(6)  and  CIO(l)  as  in  the  initialisation  call. 

If  110(2)  is  set  to  5,  a  "tilt"  is  applied  to  the  navigators  analytical  axes.  The 
tilt  is  specified  by  yaw,  elevation,  and  bank  changes  of  the  Body  axes.  Values  of 
these  angles  must  be  available  in  DIN(18),  DIN(17)  and  D1N(16)  respectively.  SINSl 
then  returns  without  further  processing. 

If  110(2)  is  set  to  6,  a  "tilt"  is  applied  as  above,  except  that  it  is  specified  as 
changes  relative  to  Geographic  axes. 

nOO)  is  not  used. 

110(4)  and  110(5)  -  same  as  initialisation  call. 

110(6)  is  not  used,  except  as  above  with  110(2)  set  to  4. 

110(7)  and  110(8)  are  outputs  -  same  as  initialisation  call. 
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CIO(l)  is  not  used,  except  as  above  with  110(2)  set  to  4. 

CIO(2)  is  not  used. 

The  DIN  are  all  inputs  to  SINSl: 

DIN(l)  to  DIN(6)  contain  body  axes  components  of  angular  velocity  and 
specific  force,  or  their  integrals. 

DIN(7)  contains  HEIGHT. 

DIN(8)  and  higher  are  not  used,  except  that  DIN(18),  DIN(17)  and  DIN(16)  may 
be  used  for  yaw,  elevation,  and  bank  angles  respectively  for  tilt  introduction  (where 
110(2)  is  set  to  5  or  6). 

5.3  Output  returned  by  SINSl 

According  to  the  value  of  110(5)  -  OUTFLAG,  SINSl  returns  either  no  output 
(0),  partial  output  (1),  or  full  output  (2).  Table  3  lists  the  output  quantities  in  DOUT. 

Partial  output  fills  DOUTd)  to  DOUT(28):  full  output  also  fills  DOIJT(29)  to 
DOUT(38).  In  partial  output,  DOUTd)  is  a  time  tag  for  D0UT(2)  to  DOUT(22),  and 
DOUT(23)  is  a  time  tag  for  DOUT(24)  to  DOUT(28).  In  the  full  output  case,  DOUTd) 
is  the  time  tag  for  all  output. 

5.4  Sensors’  characteristics  data  file  format 

The  full  pathname  of  this  data  file  should  be  placed  in  ClOd).  If  ClOd)  is  null 
string,  default  values  will  be  used. 

An  example  of  contents  of  the  data  file  is: 

(These  are  the  default  values,  which  assume  the  system  is  error  free.) 

0  /♦  Gyro  misalignment 

1.0  0.0  0.0 
0.0  1.0  0.0 
0.0  0.0  1.0 

0  /*  Acc  misalignment 

1.0  0.0  0.0 
0.0  1.0  0.0 
0.0  0.0  1.0 

1.0  1.0  1.0  1.0  1.0  1.0 


/*  Gyros,  Accs,  s/factors 
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0.0  0.0  0.0 

0.0  0.0  0.0 

/*  Gyros,  Accs,  biases 

0.0  0.0 

/*  Gyros,  Accs,  quant,  levels 

Names  of  the  corresponding  variables  in  the  program  are: 

GYMIS 

GYMXll 

GYMX12 

GYMX13 

GYMX21 

GYMX22 

GYMX23 

GYMX31 

GYMX32 

GYMX33 

ACMIS 

ACMXll 

ACMX12 

ACMX13 

ACMX21 

ACMX22 

ACMX23 

ACMX31 

ACMX32 

ACMX33 

GYSF(l) 

GYSF(2) 

GYSF(3)  ACSF(l)  ACSF(2)  ACSF(3) 

GYBIAS(l) 

GYBIAS(2)  GYBIAS(3)  ACBIAS(l)  ACBIAS(2)  ACBIAS(3) 

GQLEVEL  AQLEVEL 

In  the  above,  the  first  letter  being  G  or  A  denotes  gyro  or  accelerometer 
respectively.  GYMIS  and  ACMIS  are  integer,  all  others  are  real. 

GYMIS  is  used  to  indicate  if  any  misalignments  exist  in  the  misalignment  matrix 
GYMX.  If  it  is  0,  perfect  alignment  is  assumed,  and  the  values  in  the  components  of 
GYMX  are  not  used,  although  they  must  be  in  the  data  file.  For  any  other  value,  the 
GYMX  are  used. 

GYMX  are  misalignment  factors.  GYMXjk  means  that  the  gyro  on  the  j  axis  senses 
GYMXjk  times  the  integral  of  body  rate  about  the  k  axis. 

GYSF  are  scale  factor  errors.  The  output  of  the  gyro  on  the  j  axis  is  multiplied  by 
GYSF(j). 

GYBIAS(j)  are  fixed  biases,  in  degrees  per  hour.  These  are  converted  to  radians  per 
sensor  cycle,  and  added  to  the  respective  gyro’s  integrated  output. 


GQLEVEL  is  the  quantization  level  for  all  gyros,  in  arcseconds.  This  may  take  a  zero 
value,  in  which  case  the  quantization  level  is  the  resolution  (double  precision)  of  the 
computer  used  to  run  the  program. 
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Accelerometer  characteristics  are  similarly  described,  except  that  the 
misalignments  operate  on  the  integrals  of  specific  force,  and  the  units  fcr  fixed 

o 

biases  are  metres  per  second  ,  and  quantization  is  in  metres  per  second. 

5.5  Navigator  initialisation  data  file  format 

The  full  pathname  of  this  data  file  should  be  placed  in  CIO(2).  If  CIO(2)  is  a 
null  string,  default  values  will  be  used. 

An  example  of  contents  of  the  data  file  is: 

6378135.0  298.26  0.0000729211515  9.780333  0.0052884  2.014 
2  16  32 

10  10  7  1 

Names  of  the  corresponding  variables  in  the  program  are: 

EQRAD  INVELL  ERATE  EQATGRAV  GRAVLAT  GRAVALT 

SSPERFAST  SSPERIMED  SSPERSLOW 

QWBNORM  QEWNORM  QUPORDER  AP9MOD 

The  first  line  consists  of  Earth  quantities.  They  are  all  real. 

EQRAD  is  the  equatorial  radius  in  metres 

INVELL  is  inverse  of  ellipticity 

ERATE  is  Earth  rate  in  radians  per  second 

2 

EQATGRAV  is  gravity  at  the  equator  in  metres  per  second 
GRAVLAT  is  a  factor  for  variation  of  gravity  with  latitude 

GRAVALT  is  a  factor  for  variation  of  gravity  with  altitude 

The  second  line  are  simulation  steps  (increments  of  SIMSTEPCTR)  per 
navigation  algorithm  step.  They  are  all  integers. 

SSPERFAST  is  the  number  of  steps  per  call  to  NAVFAST.  It  must  be  an  integer 
multiple  of  the  sensor  cycle  period,  which  is  either  1  or  2  simulation  steps  in  length, 
according  to  the  value  of  SENSTEP.  The  Fast  period  is  double  the  time  between 
calls  to  NAVFAST. 
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SSPERIMED  is  the  number  of  steps  per  call  to  NAVIMED.  It  must  be  an  integer 
multiple  of  the  fast  period,  that  is,  it  must  be  an  even  multiple  of  SSPERFAST. 

SSPERSLOW  is  the  number  of  steps  per  call  to  NAVSLOW.  It  should  be  an  integer 
multiple  of  SSPERIMED. 

The  third  line  quantities  are  integers. 

QWBNORM  is  the  minimum  number  of  times  QWB  may  be  updated  between 
normalizations.  This  occurs  in  NAVIMED  only,  so  if  a  large  number  of  fast  cycles 
are  performed  for  each  intermediate  cycle,  QWB  may  be  updated  that  many  more 
times  before  normalization. 

QEWNORM  is  the  number  of  times  QEW  is  updated  between  normalizations. 
QUPORDER  is  the  order  of  calculation  of  the  updating  quaternion  in  NAVFAST.  A 
minimum  of  3rd  order  is  always  used.  If  QUPORDER  is  greater  than  3,  then  5th 
order  is  used;  if  it  is  greater  than  5,  then  7th  order  is  used.  The  extra  computing 
load  for  higher  orders  is  almost  negligible. 

AP9MOD  is  a  flag  for  higher  order  body  axes  velocity  calculation  in  NAVFAST.  If  it 
is  0,  a  simple  calculation  is  performed:  otherwise  a  4th  order  Runge-Kutta  is  used. 
The  extra  computing  burden  may  be  significant. 

Values  listed  above  are  the  default  values  for  the  first  and  the  third  lines  of 
data.  The  default  for  SSPERFAST  is  the  value  of  SENSTEP,  i.e.,  110(2)  on  the  first 
call.  The  default  for  SSPERIMED  is  SSPERFAST  times  8,  which  means  the 
intermediate  period  is  4  times  the  fast  period.  The  default  for  SSPERSLOW  is  equal 
to  SSPERIMED. 

6.  HANDLER  PROGRAM  EXAMPLE:  FPSIN2 

This  section  contains  a  description  of  an  example  of  a  handler  program  known 
as  FPSIN2,  which  operates  SINSl  using  the  outputs  of  a  flight  profile  generator 
program  FPG2  (reference  3). 

FPG2  generates  a  file  containing  a  sequence  of  integrated  (in  body  axes) 
specific  forces  and  angular  velocities  at  a  point  in  an  aircraft  executing  a  sequence 


of  user  defined  manoeuvres.  They  are,  effectively,  "perfect"  IMU  outputs.  It  also 
generates  a  "reference"  file  containing  a  sequence  of  nominally  true  position, 
velocity,  and  attitude  of  the  aircraft,  and  a  file  of  initialisation  data  for  use  by 
FPSIN2. 

FPSIN2  functions  as  a  time  stepping  simulation.  At  each  step,  it  reads  the 
FPG2  file  of  integrated  sensor  data  and  passes  these  to  SINSl.  As  its  output,  SINSl 
provides  the  strapdown  INS  estimates  of  position,  velocity,  and  attitude  -  the  system 
state.  At  specified  time  intervals,  FPSIN2  obtains  this  output.  It  also  reads  the 
reference  file  data  generated  by  FPG2.  Errors  in  the  INS  estimates  of  system  state 
are  then  calculated  and  written  to  diskfiles  and/or  the  terminal. 

6.1  Program  Description 

The  main  module  of  the  handler  program  is  called  FPSIN2,  as  shown  in  figure 
4.  It  performs  initialisation,  then  steps  through  the  simulation. 

During  initialisation  it  calls  FPSFILES  and  INISINSl.  FPSFILES  is  a  file 
handler  which  opens  and  reads  the  initialisation  data  files,  opens  the  FPG2  IMU  and 
reference  data  files  for  reading  during  the  run,  prepares  files  for  output,  and  does 
checking  and  manipulation  of  the  initial  input  data.  The  timing  variables  and 
counters  are  set  to  zero.  INISINSl  is  a  SINSl  initialisation  handler.  SINSl  is  to  use 
integrated  sensor  data,  and  full  output  from  SINSl  is  required  at  the  initialisation 
call,  so  flags  are  set,  and  INISINSl  is  called.  It  sets  up  the  call  arguments  for  the 
initialisation  call  to  SINSl,  calls  it  and  obtains  its  output. 

In  each  step  of  the  run,  the  stepcounter  is  incremented,  and  routines  CLOCK, 
RUNSINSl,  and  FPSOUT  are  called.  CLOCK  is  a  timing  utility  which  keeps  time  in 
hours,  minutes,  and  seconds.  RUNSINSl  is  a  SINSl  run-time  handler.  Before  this  is 
called,  FPSIN2  checks  the  stepcounter  to  ascertain  if  output  is  required,  and  sets  a 
flag  accordingly.  RUNSINSl  is  then  called.  This  reads  the  FPG2  IMU  data  and  sets 
up  the  run  arguments  for  the  call  to  SINSl,  calls  it  and  gets  its  output.  The  output 
routine  FPSOUT  is  entered  on  every  step,  and  generates  and  writes  output  if  required 
(as  determined  from  the  stepcounter). 

Finally,  when  stepping  is  completed,  all  open  files  are  closed. 
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6.2  Input  Files  and  Initialisation 

The  first  file  sought  by  FPSFILES  is  a  command  file  which  contains  the 
following  information,  one  item  per  line,  in  the  order  shown: 

Directory  name  where  FPG2  output  files  are  to  be  found 

The  filename  suffix  xxx  of  the  FPG2  files  to  be  used 

Second  filename  suffix  yyy  to  be  used  for  FPSIN2  output 

(Existing  directory  name  where  FPSIN2  output  files  are  to  be  written) 

Terminal  output  specifier. 

This  command  file  is  to  have  the  name  FPSRUN,  and  is  to  be  in  the  users 
directory  from  which  FPSIN2  is  being  executed.  If  this  file  cannot  be  found,  FPSIN2 
will  ask  for  the  same  data  interactively  at  the  terminal. 

Data  files  required  to  have  been  generated  by  FPG2  (the  "flight  specific"  files), 
and  located  in  the  directory  specified  above,  are: 

SIMDATA.xxx  the  file  of  initialisation  data  for  FPSIN2 

IMUOUT.xxx  the  file  of  "perfect”  IMU  sensor  data 

REFOUT.xxx  the  file  of  "reference"  flight  profile  data. 

These  files  should  not  be  altered  by  the  user.  SIMDATA.xxx  contains  initial  true 
values  of  position,  velocity,  attitude,  and  wander  angle,  plus  timing  data. 

A  "run  specific"  input  data  file  of  INS  initialisation  data  is  also  required.  This 
is  prepared  by  the  user,  is  to  be  in  his  directory  as  above,  and  has  the  name 
INSDATA.xxx.yyy  where  the  xxx  and  yyy  are  specified  as  above. 

6.2.1  INS  Initialisation  Data  File  INSDATA.xxx.3ryy 

Contents  of  this  file,  using  variable  names  as  in  the  program,  are: 

ELON  ELAT  EALPHA  EHEIGHT  EVNORTH  ...  (continued) 

...  EVEAST  EVDOWN  EBANK  EELEV  EHEAD 

UNI 

FNINSSEN 

FNINSNAV 

ALYNFLAG  HGHTFLAG. 
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The  first  line  is  a  set  of  initial  error  quantities:  longitude,  latitude,  wander 
angle  (degrees);  height  (metres  +  up);  velocities  north,  east,  down  (metres/second); 
attitudes  bank,  elevation,  heading  (degrees).  These  are  used  in  setting  up  the 
simulated  INS.  The  actual  value  used  by  the  TNS  for  any  variable  is  the  reference 
value  (from  SIMDATA)  plus  this  error  value. 

UNI  is  a  Fortran  unit  number  for  use  by  SINSl.  It  may  be  any  integer  greater 
than  8  available  to  the  user  (FPSIN2  uses  1  to  8). 

FNINSSEN  and  FNINSNAV  are  the  pathnames  (up  to  80  characters  each)  for 
SINSl  sensor  characteristics  data  and  navigator  data  files  respectively.  Contents  of 
these  are  discussed  in  a  previous  section.  If  either  has  the  value  ’  ’  its  default  is 
used. 

ALYNFLAG  and  HGHTFLAG  are  alignment  and  height  flags  respectively,  and 
each  may  have  values  0  or  1.  If  ALYNFLAG  is  1,  the  coarse  one-shot  INS  alignment 
will  be  carried  out;  if  it  is  0,  the  data  values  of  heading,  bank,  and  elevation  will  be 
used.  If  HGHTFLAG  is  0,  the  reference  values  of  height  will  be  used  by  SINSl,  and 
its  vertical  velocity  output  will  be  set  *o  zero;  if  it  is  1,  the  vertical  channel  is  free 
inertial  (and  therefore  unstable). 

If  a  file  INSDATA.xxx.yyy  cannot  be  found,  FPSFILES  looks  for  a  file 
INSDATA.xxx,  and  if  this  cannot  be  found,  it  looks  for  a  file  INSDATA. 

6.3  Initialisation  Call  to  SINSl 

INISINSl  sets  up  the  call  arguments  (see  Section  5)  for  the  initialisation  call, 
namely,  nOd  to  6),  ClOd  and  2),  DIN(7  to  15)  and  DIN(16  to  28)  as  required.  FPSIN2 
sets  SENSFLAG,  for  110(2)  to  1,  and  OUTFLAG,  for  110(5)  to  2  ( "Full”  SINSl  output  is 
required  here),  and  the  stepcounter,  for  nOd),  to  zero.  Other  arguments  are  all 
available  from  the  initial  data  files,  either  directly  or,  for  INS  initial  estimate 
conditions,  by  adding  the  error  and  reference  quantities. 

6.4  Runtime  Call  to  SINSl 

RUNSINSl  reads  the  FPG2  IMU  data  and  sets  up  the  run  arguments  for  each 
call  to  SINSl,  namely  110(1,2,4  and  5)  and  DINd)  to  DIN(7).  nOd  and  5)  are 
determined  by  FPSIN2,  110(2  and  4)  are  from  the  initial  data  files.  The  DIN  values 
come  straight  from  the  I”PG2  IMU  data  file. 
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6.5  Output  from  FPSEN2 

The  following  files  are  written,  in  the  directory  specified  in  the  command  file 
or  interactive  session  with  FPSFILES: 

ERRORS. xxx.yyy  file  of  INS  errors 

RANGES. XXX. yyy  maximum  and  minimum  values  in  above 

RECORD. xxx.yyy  record  file  of  all  initialisation  data. 

The  file  ERRORS.xxx.yyy  is  an  ASCII  file  of  data  in  columns: 

TIME  LON  LAT  ALPHA  HEIGHT  VNORTH  VEAST  VDOWN  BANK  ELEV  HEAD. 

Each  line  is  the  difference  between  the  INS  estimate  and  the  reference  value  of  the 
various  quantities  at  the  time. 

The  file  RANGES. xxx.yyy  is  similar  to  ERRORS.xxx.yyy,  except  that  only  the 
greatest  and  least  of  each  quantity  is  recorded. 

The  file  RECORD. xxx.yyy  contains  a  list  of  the  contents  of  the  SIMDATA.xxx 
file,  the  INSDATA. xxx.yyy  file,  and  the  contents  of  the  SINSl  sensor  data  and 
navigator  data  files. 

If  the  terminal  output  specifier  is  "F",  the  same  data  is  sent  to  the  terminal  as 
is  written  in  the  ERRORS.xxx.yyy  file.  If  the  specifier  is  "P",  the  only  data  sent  to 
the  terminal  are  the  times  at  which  each  FPG2  manoeuvre  is  completed,  and  may 
provide  reassurance  that  the  computer  is  still  operating.  Otherwise  there  is  no 
terminal  output. 

7.  AREAS  FOR  DEVELOPMENT 

In  its  present  form,  the  program  may  use  a  rather  idealised  one  shot  open  loop 
alignment.  In  a  real  INS,  with  noise  and  quantization  in  the  sensors  and  movement  of 
the  system,  this  arrangement  would  not  be  satisfactory,  except  for  a  coarse  levelling 
and  perhaps  a  rough  gyrocompassing.  There  is  scope  for  inclusion  of  an  alignment 
phase:  this  could  use  much  of  the  existing  program  structure,  probably  with  the 
addition  of  an  alignment  status  in  NAVEXEC,  which  would  call  an  alternative 
intermediate  rate  routine  to  perform  the  alignment  functions. 

A  realistic  alignment  routine  was  not  included  -n  this  program  because  it  was 
intended  for  use  as  a  simulation  of  a  navigating  system,  in  which  certain 
initialisation  errors  may  be  included  as  required.  In  practice,  an  alignment  routine 
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would  in  any  case  be  designed  to  suit  the  characteristics  of  a  particular  system  and 
application. 

The  vertical  channel,  if  not  in  pure-inertial  mode,  relies  upon  provision  of 
accurate  height  data  to  the  system,  from  which  vertical  velocity  is  derived.  A  useful 
development  would  be  the  inclusion  of  a  more  realistic  vertical  channel,  which  could 
be  inserted  in  NAVIMED  at  the  point  where  the  vertical  velocity  is  calculated. 

Sensor  error  models  could  be  further  developed  if  required  by  inclusion  of  the 
appropriate  characteristic  into  SENSORS  and  SENREAD,  and  if  initialisation  is 
required,  into  SENSINI  also.  Note  that  if  any  additional  modelled  errors  are  likely  to 
affect  a  one-shot  alignment,  they  should  also  be  included  in  SNAPALYN.  Sensor 
error  characteristics  were  repeated  separately  in  SNAPALYN  in  order  to  keep  the 
Sensors  segment  and  the  Navigator  segment  structurally  separated  in  the  total 
program. 
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TABLE  1:  Integer  and  Character  Areruments  to  SINSl  Call 

When  read  or  written 


Function 

Integer  Input/Output  array  HO. 

Input  to  SINSl: 

nCKl)  Step  counter 

110(2)  Sensors /tilts  flag 

nOO)  Alignment  flag 

110(4)  Height  flag 

110(5)  Output  flag 

110(6)  Data  file  unit 


-  every  call 

-  every  call 

-  first  call 

-  every  call 

-  every  call 

-  first  call  or  when  110(2)  =  4 


-  when  110(5)  =  1  or  2 

-  every  call 


-  first  call  or  when  110(2)  =  4 
-  first  call  only 


Output  from  SINSl: 

110(7)  Output  flag 

110(8)  Status  flag 

Also,  nOO)  and  nOdO)  are  available  but  unused. 

Character  string  input  array  CIO. 

CIO(l)  Pathname  of  sensor  dnta  file 
CIO(2)  Pathname  of  navigator  data  file 

Also,  CIO(3)  and  CI0(4)  are  available  but  xinused. 


DIN(l) 

Angular  velocity  component  - 

axis  B1 

radians /sec 

DIN(2) 

ditto 

axis  B2 

ditto 

DIN(3) 

ditto 

axis  B3 

ditto 

DIN(4) 

Specific  force  component  - 

axis  B1 

metres/sec^ 

DIN(5) 

ditto 

axis  B2 

ditto 

DIN(6) 

ditto 

axis  B3 

ditto 

(if  integrals  of  the  above  are  used,  imits  are  radians  and  metres/sec.,  and  they  need 
not  be  defined  for  the  first  call) 

DIN(7)  Height 

Fiist  call  only: 

metres  (+up) 

DIN(8) 

Simulation  time  between  calls 

seconds 

Initial  conditions  for  navigator: 


DIN(9) 

Longitude 

radians 

DIN(IO) 

Latitude 

radians 

DIN(ll) 

Wander  angle 

(  +ve  G  to  W  axes) 

radians 

DIN(12) 

Height 

(  +ve  up) 

metres 

DIN(13) 

North  velocity 

( +ve  N ) 

metres/sec 

DIN(14) 

Blast  velocity 

( +ve  E ) 

metres/sec 

DIN(15) 

Down  velocity 

(  +ve  D ) 

metres/sec 

DIN(16)* 

Bank  angle 

(  +ve  G  to  B  axes) 

radians 

DIN(17)* 

Elevation  angle 

(  +ve  G  to  B  axes) 

radians 

DIN(18)* 

Heading  angle 

(  +ve  G  to  B  axes) 

radians 

*  Only  required  if  110(3)  =  0 

True  initial  conditions.  These  are  only  required  if  110(3)  =  1;  those  marked  with  *  are 
not  required  in  any  case.  Units,  etc.,  as  above: 

DIN(19)*  Longitude 
DIN(20)  Latitude 
DIN(21)*  Wander  angle 
DIN(22)  Height 
DIN(23)*  North  velocity 
DIN(24)*  East  velocity 
DIN(26)*  Down  velocity 
DIN(26)  Bank  angle 
DIN(27)  Elevation  angle 
DIN(28)  Heading  angle 

DIN(29)  to  DIN(40)  are  not  used. 


Calls  to  introduce  tilt  changes: 

If  110(2)  is  set  to  6  or  6,  values  of  Bank,  Elevation,  and  Yaw  angles  are  required  in 
DIN(16)  to  DIN(18).  Units  are  radians. 


TABLE  3:  Double  Precision  Real  Output  Arguments  to  SINSl  Call 
If  110(5)  =  0,  SINSl  does  not  return  any  valid  output. 

If  110(5)  =  1,  SINSl  returns  partial  output: 


DOUTd) 

is 

WBTIME 

Time  tag  DOUT(2)  to  (22) 

seconds 

DOUT(2) 

is 

VEWl 

Wander  axes  velocity 

W1 

metres/sec 

DOUT(3) 

is 

VEW2 

n 

W2 

H 

DOUT(4) 

is 

VEW3 

n 

W3 

II 

DOUT(5) 

is 

HEIGHT 

Height 

metres  (+ve 

DOUT(6) 

is 

QEWO 

E  to  W  quaternion 

DOUT(7) 

is 

QEWl 

H 

DOUTXS) 

is 

QEW2 

H 

DOUT(9) 

is 

QEW3 

It 

DOUTdO) 

is 

OQWBO 

W  to  B  quaternion 

DOUTdl) 

is 

OQWBl 

DOUT(12) 

is 

OQWB2 

n 

DOUT(13) 

is 

0QWB3 

ft 

DOUT(14) 

is 

CBWll 

B  to  W  direction  cosine 

DOUT(15) 

is 

CBW12 

tt 

DOUT(16) 

is 

CBW13 

It 

DOUTd?) 

is 

CBW21 

It 

DOUTdS) 

is 

CBW22 

It 

DOUT(19) 

is 

CBW23 

It 

DOUT(20) 

is 

CBW31 

tt 

DOUT(21) 

is 

CBW32 

tt 

DOUT(22) 

is 

CBW33 

It 

DOUT(23) 

is 

EWTIME 

Time  tag  DOUT(24)  to  (28) 

seconds 

DOUT(24) 

is 

CEW13 

E  to  W  direction  cosine 

DOUT(25) 

is 

CEW23 

tt 

DOUT(26) 

is 

CEW31 

ft 

DOUT(27) 

is 

CEW32 

tt 

DOUT(28) 

is 

CEW33 

It 

If  110(5)  =  2,  SINSl  returns  full  output.  DOUTd)  is  the  time  tag  for  all  output.  In 
addition  to  the  above,  there  is: 


DOUT(29) 

is 

LON 

Longitude 

Radians 

DOUT(30) 

is 

LAT 

Latitude 

M 

DOUT(31) 

is 

ALPHA 

Wander  angle 

11 

DOUT(32) 

is 

HEIGHT 

Height 

metres  (+ve  up) 

DOUT(33) 

is 

VNORTH 

North  velocity 

metres/sec 

(+ve  N) 

DOUT(34) 

is 

VEAST 

EJast  velocity 

M 

(+ve  E) 

DOUT(35) 

is 

VDOWN 

Down  velocity 

II 

(+ve  D) 

DOUT(36) 

is 

BANK 

Bank  angle 

radians  (+ve  G  to  B) 

DOUT(37) 

is 

ELEV 

Elevation  angle 

II 

DOUT(38) 

is 

HEAD 

Heading  angle 

II 

DOUT(39)  and  DOUT(40)  are  not  used. 


ENTER 


initialisation 


SET  COUNTERS. 
FLAGS.  ETC. 


WRITE  WARNING 
SET  ERROR  FUG 


UPDATE 

SAMPLE 


OPS 

NORMAL? 


COUNT  OK? 

■x 

/WRY  T0\ 
UPDATE  SENSOR 
s^SAMPLE  ONLY?^ 


•^ENTRY^ 
TO  CHANGE 
*^TILT?^ 


WRITE 

OPERATIONAL 

STATUS 


OPS 

NORMAL? 


WRITE 

warning 
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NAVEXEC 
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Appendix  1  -  Notation  and  Coordinate  Frames 


U. 

[U]^ 


a  vector 

vector  U  in  X  frame  coordinates 


dUl 


rate  of  change  of  IJ  with  respect  to  frame  X 
X 


dU' 

Y 

dU 

dU 

dt 

X 

is 

dt 

L  J 

in  Y  frame  coordinates.  N.B. 
X 

dt 

QXY  or  Qj^ 
CXYor 

^XY 

-XY 

V 

-XY 

£. 

°E 

(*) 


quaternion  representing  rotation  from  X  to  Y  frame 
direction  cosine  matrix:  X  to  Y  coordinates 
"rotation  vector":  X  frame  to  Y  frame 
angular  velocity  of  Y  frame  relative  to  X  frame 
velocity  of  Y  frame  relative  to  X  frame 
gravity  vector 

scalar  Earth  rotation  rate  about  polar  axis 
quaternion  "multiplication"  operator 


Coordinate  Frames 

Axes  sets  are  defined  as  follows: 

INERTIAL  (I)  -  having  13  along  Earth’s  axis  of  rotation.  There  is  no  need  to 
define  II  and  12. 


EARTH  (E)  -  has  E3  along  Earth  axis  of  rotation,  El  and  E2  are  in  the 
equatorial  plane,  with  El  at  0  degrees  longitude. 

GEOGRAPHIC  <G)  -  having  G1  and  G2  as  local  level  North  and  East,  and  G3  is 
Down.  The  origin  is  at  the  INS  reference  point. 

WANDER  AZIMUTH  <W)  -  is  obtained  by  a  positive  rotation  of  the  G  set  about 
Down,  through  the  Wander  Angle.  The  origin  is  at  the  INS  reference  point. 

BODY  (B)  -  is  the  set  of  nominal  INS  axes:  if  the  INS  and  its  sensors  were 
perfect,  the  Body  axes  would  be  the  sensors’  sensitive  axes.  The  origin  is  at  the  INS 
reference  point. 


APPE3NDIX  2  -  Direction  Cosine  and  Quaternion  Relationships. 


There  are  many  statements  of  these  relationships  (see  for  example, 
reference  1),  they  are  repeated  here  for  convenience. 


A  2.1  Quaternion /Direction  Cosines 

Consider  a  quaternion  QXY  representing  the  rotation  from  the  X  to  the  Y 
frame,  having  components  QO,  Ql,  Q2  and  Q3.  Consider  also  the  direction  cosine 
matrix  CYX,  which  may  be  used  to  transform  coordinates  of  a  vector  from  Y  to  X 


axes. 


CYX  may  be  written 

For  a  vector  V:  [  V  ] ^ 

The  components  of  CYX  are: 

CYXll 

CYX12 

CYX13 

CYX21 

CYX22 

CYX23 

CYX31 

CYX32 

CYX33 

Given  CYX,  then  CXY  is  its  transpose. 


QO 

1 

QO 

If  QXY  is 

Ql 

Q2 

“  J. 

’  then  QYX  is  QXY 

-Ql 

-Q2 

Q3 

-Q3 

In  terms  of  the  QXY  components,  CYX  is: 


1  -  2(Q2='  +  Q3M 
2(Q0Q3  +  Q1Q2) 
2(Q1Q3  -  Q0Q2) 


2(QlQ2  -  Q0Q3) 

1  -  2(Q12  +  Q3^ ) 
2(Q0Q1  +  Q2Q3) 


2(Q0Q2  +  Q1Q3) 
2(Q2Q3  -  QOQl) 

1  -  2(Q12  +  Q2M 


A  2.2  Quaternion  "Multiplication" 

Consider  a  body  with  a  quaternion  A  =  (Aq  ,  A)  representing  the  rotation 
from  reference  to  body  axes.  Give  the  body  a  rotation  such  that  quaternion 
B  =  (Bq  ,B)  represents  the  rotation  from  old  to  new  body  axes.  The  quaternion 
C  =  (Cq.C)  representing  the  rotation  from  reference  to  new  body  axes  is: 


C  =  A(*)B  =  (AoBo  -  A.B),  (AqB  +  +  AxB) 


if  A 


and  B 


then 


AO 

A1 

A2 

A3 


BO 

B1 

B2 

B3 


CO 

AOBO  -  AlBl  -  A2B2  -  A3B3 

Cl 

= 

AOBl  +  AIBO  +  A2B3  -  A3B2 

C2 

A0B2  -  A1B3  +  A2B0  +  A3B1 

C3 

A0B3  +  A1B2  -  A2B1  +  A3B0 

also,  QRD  =  [[[  QEL'^(*)QAB  ]  (*)QBC  ]  ( *  )QC3D  1 


etc . 


Appendix  3  -  Earth  to  Geographic  to  Wander  Azimuth  Transformation. 


Rotations  are  positive  in  the  sense  E  to  G  to  W. 


A  3.1  Direction  Cosines/Euler  Angles. 


a)  rotate  about  E3 
through  Longitude  (L): 


cos(L) 

sin(L) 

o' 

DCMl  = 

-sin(L) 

cos(L) 

0 

0 

0 

1 

b)  rotate  about  G2  (East) 
through  -(90  +  latitude  (D): 


-sin(l) 

0 

cos(l) 

DCM2  = 

0 

1 

0 

-cos(l) 

0 

-sin(l) 

c)  rotate  about  G3  (Down) 
through  wander  angle  (A); 


DCM3  = 


cos(A) 

-sin(A) 

0 


sin(A) 

cos(A) 

0 


0 

0 

1 


p 

combining  these,  the  E  to  G  DCM,  Cg  =  DC3M2  .DCMl ,  is: 


-sin(l)cos(L) 

-sin(L) 

-cos(l)cos(L) 


-sin(l)sin(L) 

cos(L) 

-cos(l)sin(L) 


cos(l) 

0 

-sin(l) 


the  E  to  W  DCM, 


DCM3.C^ 


is,  (putting  c  for  cos  and  s  for  sin): 


-c(A)s(l)c(L)-s(A)s(L) 

s(A)s(l)c(L)-c(A)s(L) 

-c(l)c(L) 


-c(A)s(l)s(L)+s(A)c(L) 

s(A)s(l)s(L)+c(A)c(L) 

-c(l)s(L) 


c(A)c(l) 

-s(A)c(l) 

-s(l) 


A  3.2  Quatemions/Euler  Angles 


The  quaternion  equivalents  of  the  above  may  be  written: 


C08^(L) 

cos')fe(90+l  ) 

C08^( A) 

0 

0 

s in^(L) 

Q2  = 

0 

-sin'i(90+l ) 
0 

Q3  = 

0 

0 

s in^(A) 

combining  these  by  quaternion  "multiplication"  (appendix  2): 


QEW  =  [  Q1(*)Q2  ](*)Q3  = 


cos^(90+l )cos^(L+A) 
si n^ <90+1)8 in^(L-A) 
-sin^(90+l )co8^(L-A) 
CO s^ (90+1 ) 8 in%(L+A) 


From  a  comparison  of  the  C!j,  equation  above  with  the  quaternion  expansion  in 
appendix  2,  it  is  found  that: 


tan(LON) 

= 

-CEW32 

=  -(QEW2.QEW3-QEW0.QEW1) 

-CBW31 

-<QEW1.QEW3+QEW0.QEW2) 

sin(LAT) 

= 

-CEW33 

=  1  -  2(QEW1*  +  QEW2*) 

and  tan(A) 

= 

-CEW23 

=  -(QEW0.QEW1+QEW2.QEW3) 

CEW13 

«3EW1.<3EW3-QEW0.QEW2) 

except  at  the  poles,  where  LAT  and  A  are  undefined. 


Appendix  4  -  Navigation  (G  or  W)  to  Body  Transformation. 


Rotations  are  positive  in  the  sense  Navigation  to  Body.  The  wander  angle 
here  is  defined  clockwise  positive  about  Down,  so  Yaw  is  defined: 


Heading  =  Wander  angle  +  Yaw 


A  4.1  Direction  Cosines/Euler  Angles. 


a)  rotate  about  Down 

cos(Y) 

8in(Y) 

0 

through  Yaw  (Y): 

DCMl  = 

-sinCY) 

cosCY) 

0 

0 

0 

1 

b)  rotate  about  new  elev. 

cos(E) 

0 

-sin(E) 

through  Elevation  (E): 

DCM2  = 

0 

1 

0 

sin(E) 

0 

cos(E) 

c)  rotate  about  Bank 

1 

0 

0 

through  Bank  (B): 

DCM3  = 

0 

cos(B) 

sin(B) 

0 

-sin(B) 

cos(B) 

combining  these,  the  N  to  B  DCM  is  C^=  DCM3.DCM2.DCM1,  and  the  B  to  N 
DCM,  is  the  transpose  of  that  (putting  c  for  cos  and  s  for  sin): 


c(E)c(Y)  8(B)s(E)c(Y)-c(B)s(Y) 

c(E)s(Y)  s(B)s(E)s(Y)+c(B)c(Y) 

-s(E)  s(B)c(E) 


c(B)s(E)c(Y)+s(B)s(Y) 

c(B)s(E)s(Y)-8(B)c(Y) 

c(B)c(E) 


J 


A  4^  Quaternion /Euler  Angles. 


The  quaternion  equivalents  of  the  above  may  be  written: 


cos'ife(Y) 

C08^(E) 

cos^(B) 

0 

Q2  = 

0 

Q3  = 

s in^(B) 

0 

s in^(E) 

0 

sin^CY) 

0 

■ 

0 

combining  tliese  by  quaternion  "multiplication”  (appendix  2): 


QNB  =  (Ql(*  )Q21  (*)Q3 


i.e.  QNB 


cos'')fe(Y)cos^(E)cos^(B) 
cos^(Y)cos^(E)sin^(B) 
cos^(Y)3in^(E)cos^(B) 
|^-cos^(Y)sin^(E)sin^(B) 


+  sin')fe(Y)sin'^(E)8in^(B) 
-  sin'iCYlsin^CElcos^CB) 
+  sin';fe(Y)cos')fe(E)sin':fe(B) 
+  s in^(Y)cos^(E)co8^(B) 


From  a  comparison 


of  the 


eg 


equation  above 


with  the  quaternion  expansion  in 


appendix  2,  it  is  found  that: 


tan(Y)  = 

CBN21 

=  2(QNB0.QNB3  +  QNB1.QNB2) 

CBNll 

1  -  2(QNB22  +  QNB3*) 

sin(E)  = 

-CBN31 

=  -2(QNB1.QNB3  -  QNB0.QNB2) 

tan(B) 

CBN32 

=  2(QNB0.QNB1  +  QNB2.QNB3) 

CBN33 

1  -  2(QNB1*  +  QNB2M 

except  for  Elevation  angles  of  +/-  90  degrees,  when  Yaw  and  Bank  are  undefined.  (N 
above  may  be  G  or  W) 


Appendix  6  -  Frame  Rates 


Let  Qq  be  (scalar)  Earth  rate  about  its  polar  axis: 


then 


E 


E’ 


also. 


G 


^Gf  1 E 


cos (LAT) 
0 

-  s  i  n  ( LAT ) 


0 


E’ 


and, 


W 


r^r  .E 


cos (A) cos (LAT) 
-sin(A)co8 (LAT) 
- s i n ( LAT ) 


Q 


E’ 


If  G  moves  over  the  Blarth  with  l6n  and  LAT,  then  : 


•^-BG^ 


G 


l6n.cos(LAT) 

-LAT 

-L6N.sin(LAT) 


If  W  has  (A)  and  (A)  relative  to  G,  then: 


w 


also. 


‘--EG^ 


W 


^G^'-EG^ 


therefore. 


IX^)N.  cos  (LAT)cos  (A) 
-l/)N.  cos  (LAT)  s  i  n  (A) 
-L6N.sin(LAT) 


LAT . s i n ( 
LAT. cos ( 


J 


r  _ 


r  -iW 


W 


l6n. cos (LAT) cos (A)  -  LAT.sin(A) 
-IX!)N.  cos  (LAT)  s  in(A)  -  LAT.co8(A) 
A  -  L(;)N.sin(LAT) 


In  this  particular  wander  azimuth,  A  -  l6n  .  s  i  n  ( LAT )  is  set  to  zero, 
i.e.,  the  azimuth  is  rotated  with  Elarth  Rate  Vertical. 


>  > 


If  the  G  or  W  frame  has  a  velocity  relative  to  Elarth  surface,  then: 


- 


and 


Y 

V 


w 

A  wander  azimuth  navigation  algorithm  gives  :  it  is  necessary  to  know 

W  W 

t  1  terms  of  [ Vgl  . 

If  the  local  prime  and  meridional  Earth  radii  are  Rp  and  Rm,  and  vehicle 
height  is  H,  then: 

lAt  =  V„  and  l6n  =  V„ 

N  _ E _ 

Rm+H  (Rp+H)  cos  (LAT) 


w 

therefore, 


VgCos (A) 
Rp+H 


-Vg. s in (A) 


Rp+H 


-Vj^s  in(A) 
Rm+H 


cos  (A) 


Rm+H 


Now,  [V„]®  = 


cScYe]"^. 


1 .  e  .  , 


'N 


'E 


(and  V, 


D 


Vj^,cos(A)  -  VY.sin(A) 
Vj^.sin(A)  +  Vy.cos(A) 


w 

Putting  these  values  of  and  into  = 


EW" 


r 

^^EW^  = 


-  V, 


X 


1  - 

1 

. s in(A)co8 (A)  +  V„ 

cos  ^(A)  +  sin^ (A) 

Rm+H 

Rp+H 

Y 

Rp+H  Rm+H 

-  V. 


X 


sin^(A)  +  co8*(A) 

+ 

Vy  r  1  - 

1 

Rp+H  Rm+H 

Rm+H 

Rp+H 

s in(A)cos (A) 


0 


This  may  be  written: 


r  - 


-V^X  +  V^2 
-Vj^l  +  V^DX 
0 


where:  D1 
D2 
DX 


8 in^ (A)  +  cos  ^ ( A) 

Rp+H  Rm+H 


cos  ^  (A) 
Rp+H 

_ 

Rm+H 


8  in^  (A) 
Rm+H 


_ 1_ 

Rp+H 


.  s i n (A) cos (A) 


From  reference  2, 

Rm  =  Re  ( 1  -  e  ^  ) _  and  Rp  =  _ Re _ 

(1  -  e^sin^dAT))®/^*  (1  -  e  ^  s  indLAT) )  ^  ^ 


where  Re  is  EJarth  equatorial  radius,  and  e  *  is  (Earth  eccentricity)  *  . 

Dl,  D2,  and  DX  may  be  calculated  to  whatever  degree  of  accuracy  is 
required;  for  a  first  approximation,  put: 


Rm  = 

Re  + 

rm 

i  .  e  .  , 

rm  = 

=  Re .  e 

dfsindLAT)  -1), 

(approx) 

and 

Rp  = 

Re  + 

rp 

i  .e.  , 

rp  = 

=  Re .  e 

^.pindLAT)  . 

(approx) 

Then, 

1 

1 

1  - 

ro+H 

(approx) 

Rp+H 

Re 

, 

Re 

and 

1 

1 

[l  -  rm+H 

(approx) 

Rm+H 

Re 

[ 

Re 

Substituting  these  into  Dl,  D2  and  DX,  and  rearranging: 


Dl  =  1 

1  -H  -^e^(  sin*  (LAT)  -  2cos  *  (LAT)  cos  *  (A)  )1 

Re 

Re  J 

D2  =  1 

1  -  H  -  ^e*(  sin*(LAT)  -  2cos  *  (LAT)  s  in*  (A)  )1 

Re 

Re  J 

DX  =  e  s in(A) cos (A)cos ^ (LAT) 

Re 

In  terms  of  the  CEW: 

Dl  =  1 

1  -  H_  -  '^!€*(  CEW33*  -  CEW13*  ) 

Re 

Re 

D2  =  1 

1  -  H_  -  ^e*(CEW33*  -  (3EW23*  )' 

Re 

Re 

DX  =  -el 
Re 


CEW13  .CEW23 


Appendix  6  -  Gravity  Support  Correction 


This  is  an  extension  of  Reference  (1),  appendix  10,  which  was  not  included  in 
that  publication.  Its  effect  is  to  eliminate  much  of  the  position  and  velocity  error 
seen  in  fig.  5  of  that  publication,  which  were  primarily  in  the  East-West  direction. 

From  that  appendix  10  we  have:  “SV^p  =  (  1  -  W[^jpX])6Vjp 

Where  “SYpp  is  the  velocity  increment  relative  to  Earth  caused  by  specific  force,  and 
5Vjp  is  the  velocity  increment  relative  to  "inertial  space"  caused  by  specific  force. 

Consider  that  component  of  5Vjp  caused  by  gravity  support: 


CiViFg] 


G  or  W 


0 

0 

-gT 


For  this  only, 


— EFg  <  1  -  ^T[  WjpX] )  5Vjp^ 


Now, 


W 


CEW13 

CEW23 

CEW33 


‘E 


Therefore,  EiYppg]'^  =  [iYjp^]'^  -  [  SVjp^]^ 


i.e.  CiYppg] 


-CEW23 

CEW13 

0 


So,  in  order  to  correct  for  the  effects  of  gravity  support,  we  subtract 
AXIOMOD  from  the  velocity  increment  in  Wander  axes,  where: 


(  =  ^gT^OgCos (LAT) s in(A)  ) 


AXIOMODI 

-  'ifegT*Qp.CEW23 

AX10MOD2 

= 

^gT2Qp.CEW13 

AX10MOD3 

0 

(  =  ^gT^QpCOS (LAT)cos (A)  ) 


N.B.  In  Geographic  axes,  (A  =  0),  we  only  have  AX10MOD2  =  ^gT^Q„co8  (LAT) 


Appendix  7  -  One  Shot  Coarse  Alignment. 


Simulated  sensors’  measurements  of  gravity  support  and  Earth  rate  are  used  to 
estimate  the  Wander  to  Body  quaternion  QWB.  The  system  is  static  relative  to 
Earth,  and  sensor  quantization  is  not  included.  (A  somewhat  idealized  situation!) 
The  sequence  of  operation  is: 

1)  Calculate  simulated  sensor  outputs,  using  the  known  "true"  attitude  of  the 
system  and  the  required  sensor  errors  (misalignments,  scale  factors,  fixed  biases). 

2)  Use  simulated  accelerometer  outputs  to  calculate  the  (Body  Level)  to  Body 
axes  quaternion  QLB  . 

3)  Use  QLB  to  calculate  simulated  levelled  gyro  outputs,  and  use  these  to 
calculate  the  Geographic  to  (Body  Level)  axes  quaternion  QGL . 

4)  Calculate  QGB  =  QGL(*)QLB;  use  required  wander  angle  to  get 

QTO,  then  =  QWG(*)QCT. 

A  7.1  Simulated  sensor  outputs. 

From  initialization  data. 


0 

0 

g3 


and 


G 


cos (LAT) 
0 

-sin(LAT) 


0 


E’ 


and  C!|^  may  be  obtained  from  the  "true"  attitude  angles. 

If  Ts  is  sensor  cycle  time,  then: 

"perfect"  accelerometer  output  =  -[g^I^.Ts  =  -C^[g]^.Ts 


"perfect"  gyroscope  output 


-  cg[»jE]°.Ts 


These  may  then  be  corrupted  with  sensor  errors  in  the  same  way  as  is  done  in  the 
sensors  simulation. 


A  7.2  Levelling 


Consider  an  axis,  perpendicular  to  the  plane  containing  B3  and  the  local 
vertical.  The  Body  Level  coordinate  frame  (L)  is  defined  by  rotating  the  Body  set 
about  that  axis  until  B3  is  coincident  with  Down.  The  angle  (b)  may  be  defined  as  the 
magnitude  of  that  rotation,  positive  from  Level  to  Body. 


Let  the  simulated  accelerometers  outputs  be 


A1 

A2 

A3 


these  may  be  normalized  to  1  by  dividing  each  component  by 
Al  2  +  A2  2  +  A3  2  ; 


let  the  result  be 


al 

a2 

a3 


From  figure  A  7.1,  it  may  be  seen  that  cos(b)  =  -a3. 


Consider  the  plane  containing  Bl,  B2,  OH,  and  the  levelling  axis;  as  shown  in 
figure  A  7.2,  the  angle  between  Bl  and  the  levelling  axis  is  (c).  The  unit  vector 
[£]  along  the  levelling  axis  is,  in  Body  coordinates,  given  by: 


el 

cos ( c ) 

[e]  = 

e2 

= 

s in(c) 

e3 

0 

From  the  diagram,  it  may  be  seen: 

cos(c)  =  -al  ,  s  i  n ( c )  =  a2 _ 

\/al  ^  +  a2  ^  VaT^  +  a2^ 

From  the  definition  of  the  quaternion  (eg,  reference  1,  appendix  2): 


cos^( c ) 

l-a3 

el.sin^(c) 

=  1 

-a2 

e2 .  s  in'ife(c) 

y2(l-a3) 

al 

e3 . s in^(c ) 

0 

A  7.3  Geographic  to  Body  Level  Quaternion. 

The  gyro  output  vector  is  normalized  to  its  correct  magnitude  (  Q„  .Ts  )  ,  and 

_  rj 

is  then  obtained  in  Level  axes  coordinates  using  QLB  : 


B. 


-1 


[gyros ]  =  QLB( * )[ gyros  3  ( * )QLB 


GU 

G12 

G13 


(  "Gyro  1  ^  ^ 


Let  (d)  be  the  angle  from  North  to  (Body  LeyeDl  axis,  as  in  figure  A  7.3.  It  can  be 
seen  from  the  figure  that 


cos(d)  =  Gll 

v/GII^  +  G12  2 

The  unit  vector  [e]  for  the  rotation  from  Geographic  to  Body  Level  is  on  the  Down 
axis,  i.e. 


Ce]  = 

From  the  definition  of  the  quaternion: 


QGL  = 


cos^(d) 

A(1  +  cos(d)) 

0 

3= 

0 

0 

0 

s in^(d) 

A(1  -  cos(d)) 

The  Geographic  to  Body  quaternion  may  now  be  obtained:  QGB  =  QGL(  *  )QLB . 

A  7.4  Wander  to  Body  Quaternion 

If  (A)  is  the  required  initial  wander  angle,  defined  as  positive  from  G  to  W, 


then: 


QWG  = 


cos^(A) 

0 

0 

-s in^(A) 


the  required  result  may  now  be  obtained: 


QWB 


QWG( * )QGB 
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